#include "undistort.h"

bool CUndistort::readParams()
{
    ifstream in(calibResultPath, ios::in);
    if(in.is_open() == false)
    {
        CCalibration::Log("读取文件失败:" + calibResultPath);
        //cout << "read file failed:" << calibResultPath << endl;
        return false;
    }
    YAML::Node node = YAML::Load(in);
    K.at<float>(0,0) = node["fx"].as<float>();
    K.at<float>(1,1) = node["fy"].as<float>();
    K.at<float>(0,2) = node["cx"].as<float>();
    K.at<float>(1,2) = node["cy"].as<float>();
    if(camera_type=="CV")
    {
        discoeff.at<float>(0, 0) = node["k1"].as<float>();
        discoeff.at<float>(1, 0) = node["k2"].as<float>();
        discoeff.at<float>(2, 0) = node["p1"].as<float>();
        discoeff.at<float>(3, 0) = node["p2"].as<float>();
        discoeff.at<float>(4, 0) = node["k3"].as<float>();
    
    }
    else if(camera_type=="FISHEYE")
    {

        discoeff.at<float>(0, 0) = node["k1"].as<float>();
        discoeff.at<float>(1, 0) = node["k2"].as<float>();
        discoeff.at<float>(2, 0) = node["u1"].as<float>();
        discoeff.at<float>(3, 0) = node["u2"].as<float>();
    }
    in.close();
    return true;
}

bool CUndistort::undistProcess()
{
    //***************畸变校正****************//
    R = Mat::eye(Size(3, 3), CV_32FC1);
    Mat mapx, mapy;
   // cout<<srcImgPath<<endl;
    Mat srcImg = imread(srcImgPath);
    Mat dstImg;
    if(camera_type=="CV")
    {
        cv::initUndistortRectifyMap(K, discoeff, R, K, srcImg.size(), CV_32FC1, mapx, mapy);
    }
    else if(camera_type=="FISHEYE")
    {
        cv::fisheye::initUndistortRectifyMap(K, discoeff, R, K, srcImg.size(), CV_32FC1, mapx, mapy);
    }
    remap(srcImg, dstImg, mapx, mapy, cv::INTER_LINEAR);
    cv::resize(dstImg, dstImg, cv::Size(), 0.25, 0.25, cv::INTER_LINEAR);
    cv::namedWindow("show", 1);
    imshow("show", dstImg);
    waitKey(500);
    std::string savePath = "./undistort_result.jpg";
    bool issave = imwrite(savePath, dstImg);
    if (issave) {
        CCalibration::Log("矫正图像保存成功:" + savePath);
        //std::cout << "矫正图像保存成功:" << savePath<<std::endl;
    } else {
        CCalibration::Log("图像保存失败.");
        //std::cerr << "图像保存失败." << std::endl;
    }
    return true;
}

void CUndistort::run()
{
    bool readSuccess = readParams();
    if (!readSuccess)
    {
        CCalibration::Log("读取参数失败.");
        //cout << "read Params Failed!" << endl;
        getchar();
    }
    undistProcess();
}

